
#include "convert.h"
#include <rcl_interfaces/msg/floating_point_range.hpp>
#include <rcl_interfaces/msg/parameter_descriptor.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>

#include <pcl_conversions/pcl_conversions.h>
#include "organized_cloudXYZIR.hpp"
#include "pointcloudXYZIR.hpp"

#include <cmath>
#include <functional>
#include <memory>
#include <string>

namespace lidar209_pointcloud
{
/** @brief Constructor. */
Convert::Convert(const rclcpp::NodeOptions & options)
  : rclcpp::Node("to_pointcloud_node", options),
    tf_buffer_(this->get_clock()),
    diagnostics_(this)
{
  std::string calibration_file = this->declare_parameter("calibration", "");
  RCLCPP_INFO(this->get_logger(), "get correction calibrationFile: %s", calibration_file.c_str());

  rcl_interfaces::msg::ParameterDescriptor min_range_desc;
  min_range_desc.name = "min_range";
  min_range_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
  min_range_desc.description = "minimum range to publish";
  rcl_interfaces::msg::FloatingPointRange min_range_range;
  min_range_range.from_value = 0.1;
  min_range_range.to_value = 10.0;
  min_range_desc.floating_point_range.push_back(min_range_range);
  double min_range = this->declare_parameter("min_range", 0.9, min_range_desc);

  rcl_interfaces::msg::ParameterDescriptor max_range_desc;
  max_range_desc.name = "max_range";
  max_range_desc.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE;
  max_range_desc.description = "maximum range to publish";
  rcl_interfaces::msg::FloatingPointRange max_range_range;
  max_range_range.from_value = 0.1;
  max_range_range.to_value = 200.0;
  max_range_desc.floating_point_range.push_back(max_range_range);
  double max_range = this->declare_parameter("max_range", 130.0, max_range_desc);

  std::string target_frame = this->declare_parameter("target_frame", "");
  std::string fixed_frame = this->declare_parameter("fixed_frame", "");


  std::string lidar_model = this->declare_parameter("model", std::string("L209-QS88"));
  RCLCPP_INFO(this->get_logger(), "get lidar_model: %s", lidar_model.c_str());

std::string topic_name = this->declare_parameter("topic_name", "");
RCLCPP_INFO(this->get_logger(), "Publish Lidar Data TopicName: %s", topic_name.c_str());


  data_ = std::make_unique<lidar209_rawdata::RawData>(this);

  data_->config_.model=lidar_model;
  data_->config_.max_range=max_range;
  data_->config_.min_range=min_range;
  data_->config_.calibrationFile=calibration_file;



  bool laser_hz=this->declare_parameter("laser_hz", false);
  data_->m_b_laser_hz=laser_hz;
  double rpm=this->declare_parameter("rpm", 600.0);
  data_->m_d_rpm=rpm;

  data_->m_b_updown=this->declare_parameter("updown", false);

  data_->setup();
  bool organize_cloud = this->declare_parameter("organize_cloud", true);
  if (organize_cloud) {
    container_ptr_ = std::make_unique<OrganizedCloudXYZIR>(
          min_range, max_range, target_frame, fixed_frame,
          data_->numLasers(), data_->scansPerPacket(), tf_buffer_);
  } else {
    container_ptr_ = std::make_unique<PointcloudXYZIR>(
          min_range, max_range, target_frame, fixed_frame,
          data_->scansPerPacket(), tf_buffer_);
  }

  output_ =
      this->create_publisher<sensor_msgs::msg::PointCloud2>(topic_name, 10);



  // Diagnostics
  diagnostics_.setHardwareID("Lidar Convert");
  // Arbitrary frequencies since we don't know which RPM is used, and are only
  // concerned about monitoring the frequency.
  diag_min_freq_ = 2.0;
  diag_max_freq_ = 20.0;
  diag_topic_ = std::make_unique<diagnostic_updater::TopicDiagnostic>(
        "point_raw", diagnostics_,
        diagnostic_updater::FrequencyStatusParam(
          &diag_min_freq_, &diag_max_freq_, 0.1, 10),
        diagnostic_updater::TimeStampStatusParam());

  data_->setParameters(min_range, max_range, 0, 0);

  if (lidar_model == "L209-QS88")
  {
    lidar209_scan_ =
        this->create_subscription<lidar209_msgs::msg::Lidar209ScanCH88>(
          "lidar209_packets", rclcpp::QoS(10),
          std::bind(&Convert::processScan_ch88, this, std::placeholders::_1));
    RCLCPP_INFO(this->get_logger(), "creating a subcriber ch88");

  }
  else if (lidar_model == "L209-QS176" || lidar_model == "L209-ZW-QS176")
  {
    lidar209_scan_ =
        this->create_subscription<lidar209_msgs::msg::Lidar209ScanCH88>(
          "lidar209_packets", rclcpp::QoS(10),
          std::bind(&Convert::processScan_ch176, this, std::placeholders::_1));
    RCLCPP_INFO(this->get_logger(), "creating a subcriber ch176");
  }
}



/** @brief Callback for raw scan messages. */
void Convert::processScan_ch88(const lidar209_msgs::msg::Lidar209ScanCH88::SharedPtr scanMsg)
{
  //  RCLCPP_INFO(this->get_logger(), "111");

  if (output_->get_subscription_count() == 0 &&
      output_->get_intra_process_subscription_count() == 0)   // no one listening?
  {
    return;                                     // avoid much work
  }
  container_ptr_->setup(scanMsg);
  //RCLCPP_INFO(this->get_logger(), "222");
  // process each packet provided by the driver
  for (size_t i = 0; i < scanMsg->packets.size(); ++i) {
    data_->unpack_ch88(scanMsg->packets[i], *container_ptr_);
  }
  //  RCLCPP_INFO(this->get_logger(), "333");
  // publish the accumulated cloud message
  diag_topic_->tick(scanMsg->header.stamp);
  output_->publish(container_ptr_->finishCloud());
  //RCLCPP_INFO(this->get_logger(), "Publishing a full lidar209_CH88 pointcloud");

}



/** @brief Callback for raw scan messages. */
void Convert::processScan_ch176(const lidar209_msgs::msg::Lidar209ScanCH88::SharedPtr scanMsg)
{

  if (output_->get_subscription_count() == 0 &&
      output_->get_intra_process_subscription_count() == 0)   // no one listening?
  {
    return;                                     // avoid much work
  }
  container_ptr_->setup(scanMsg);

  // process each packet provided by the driver
  for (size_t i = 0; i < scanMsg->packets.size(); ++i) {
    data_->unpack_ch176(scanMsg->packets[i], *container_ptr_);
  }

  // publish the accumulated cloud message
  diag_topic_->tick(scanMsg->header.stamp);
  output_->publish(container_ptr_->finishCloud());
 // RCLCPP_INFO(this->get_logger(), "Publishing a full lidar209_CH176 pointcloud");

}

} // namespace lidar209_pointcloud
RCLCPP_COMPONENTS_REGISTER_NODE(lidar209_pointcloud::Convert)
